#include "algoritmos.h"

vector<double> recuperarColumna(Matriz& ,int);
double norma2(vector<double> &);
Matriz calcularMatrizW(vector<double>&, int);
Matriz calcularMatrizG(Matriz&, int, int);



Matriz calcularQRReflexiones(Matriz& A){
    Matriz Q = calcularQRReflexiones(A, false);
    return Q;
}
Matriz calcularQRReflexiones(Matriz& A, bool quieroIteraciones){    /// Devuelve Q y queda en la matriz input R
    Matriz Q(A.size(),A.size());

    int iteraciones = 0;

    for(int k = 1; k <= A.size() - 1; k++){
        vector<double> x = recuperarColumna(A, k);      /// x
        x[0] = x[0] - norma2(x);                        /// u = x - alpha*e1
        double normaX = norma2(x);

        for(unsigned int i = 0; i < x.size(); i++){              /// v = u/|u|
            x[i] = x[i]/normaX;
        }
        Matriz W = calcularMatrizW(x, A.size());


        A = W.multiplicar(A);
        iteraciones++;

        if(k == 1){
            Q = W;
        }else{
            Q = Q.multiplicar(W);
        }

    }
    if(quieroIteraciones)cout << iteraciones << endl;
    return Q;
}

vector<double> recuperarColumna(Matriz& A,int k){
    vector<double> x;
    for(int i = k; i <= A.size(); i++){
        x.push_back(A.get(i,k));
    }
    return x;
}

double norma2(vector<double> & x){
    double suma = 0;
    for(unsigned int i = 0; i < x.size(); i++){
        suma = suma + pow(x[i],2);
    }
    suma = sqrt(suma);
    return suma;
}

Matriz calcularMatrizW(vector<double>& x, int m){
    unsigned int n = (unsigned int) m;

    Matriz xxt(x.size(), x.size());

    for(int i = 1; i <= xxt.size(); i++){       /// Creo la matriz 2 * (x * x(traspueto))
        for(int j = 1; j <= xxt.size(); j++){
            xxt.set(i, j, 2*x[i-1]*x[j-1]);
        }
    }

    Matriz I(x.size(), x.size());
    for(int i = 1; i <= xxt.size(); i++) I.set(i, i, 1);       /// Creo la matriz identidad


    Matriz W(x.size(), x.size());
    for(int i = 1; i <= xxt.size(); i++){       /// Calculo W = I - 2*x*x(transpuesto)
        for(int j = 1; j <= xxt.size(); j++){
            W.set(i, j, I.get(i,j) - xxt.get(i,j));
        }
    }
    Matriz result(n, n);

    for(unsigned int i = 1; i <= n; i++){       /// Creo la matriz 2 * (x * x(traspueto))
        for(unsigned int j = 1; j <= n; j++){
            if(i <= n - x.size() || j <= n - x.size()){
                if(i == j){
                    result.set(i, j, 1);
                }else{
                    result.set(i, j, 0);
                }
            }else{
                result.set(i, j, W.get(i - (n - x.size()), j - (n - x.size())));
            }
        }
    }

    return result;
}

Matriz calcularQRRotaciones(Matriz& A){
    Matriz Q = calcularQRRotaciones(A, false);
    return Q;
}
Matriz calcularQRRotaciones(Matriz& A, bool quieroIteraciones){     /// Devuelve Q y queda en la matriz inpu R

    Matriz Q(A.size(), A.size());
    int iteraciones = 0;
    for(int i = 1; i <= A.size(); i++){       /// Seteo a Q con la identidad
                Q.set(i, i, 1);
    }

    for(int i = 1; i <= A.size(); i++){
        for(int j = 1; j < i; j++){
            if(fabs(A.get(i,j)) > 0.0000001){
                iteraciones ++;
                Matriz G = calcularMatrizG(A, i, j);

                A = G.multiplicar(A);

                Q = G.multiplicar(Q);                       ///Gn*Gn-1*...*G1*I = Qt

            }
        }
    }

    Q = Q.trasponer();
    if(quieroIteraciones)cout << iteraciones << endl;
    return Q;

}

Matriz calcularMatrizG(Matriz& A, int fil, int col){

    int n = A.size();
    Matriz G(n, n);
    vector<double> x;
    x.push_back(A.get(col, col));
    x.push_back(A.get(fil, col));
    double norma2X = norma2(x);

    for(int i = 1; i <= n; i++) G.set(i,i,1);


    G.set(fil, fil, x[0]/norma2X);
    G.set(fil, col,(-1) * x[1]/norma2X);
    G.set(col, fil, x[1]/norma2X);
    G.set(col, col, x[0]/norma2X);



    return G;

}
